#!/usr/bin/env python
#
# Copyright (c) 2020 Intel Corporation
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
"""
ground truth localization. Publishes the following topics:
    /ground_truth/current_velocty (geometry_msgs::TwistStamped)
    /ground_truth/current_pose    (geometry_msgs::PoseStamped)
"""
import rospy
import math
import tf

from geometry_msgs.msg import PoseStamped, TwistStamped
from nav_msgs.msg import Odometry

pose_pub = rospy.Publisher('/ground_truth/current_pose', PoseStamped, queue_size=1)
twist_pub = rospy.Publisher('/ground_truth/current_velocity', TwistStamped, queue_size=1)


def callback(data):
    """
    callback odometry
    """
    br = tf.TransformBroadcaster()
    origin = data.pose.pose.position
    orientation = data.pose.pose.orientation
    br.sendTransform((origin.x, origin.y, origin.z),
                     (orientation.x, orientation.y, orientation.z, orientation.w), rospy.Time.now(),
                     '/base_link', '/map')

    # Pose
    pose = PoseStamped()
    pose.header = data.header
    pose.pose = data.pose.pose

    # Twist
    linear = data.twist.twist.linear
    angular = data.twist.twist.angular

    twist = TwistStamped()
    twist.header = data.header
    twist.twist.linear.x = math.sqrt(linear.x**2 + linear.y**2 + linear.z**2)
    twist.twist.angular = angular

    pose_pub.publish(pose)
    twist_pub.publish(twist)


def carla_to_autoware_localization():
    """
    main loop
    """
    rospy.init_node('carla_to_autoware_localization', anonymous=True)
    role_name = rospy.get_param('/role_name', 'ego_vehicle')
    rospy.Subscriber('/carla/{}/odometry'.format(role_name), Odometry, callback)
    rospy.spin()


if __name__ == '__main__':
    carla_to_autoware_localization()
